#!/usr/bin/env python3
from gpiozero import AngularServo
from time import sleep

class Servo():
    def __init__(self, pin, min_angle=-90, max_angle=90, min_pulse_width=0.5/1000, max_pulse_width=2.5/1000):
        """
        初始化舵机控制器
        
        参数:
            pin: GPIO 引脚号 (BCM编号)
            min_angle: 最小角度 (默认-90度)
            max_angle: 最大角度 (默认90度)
            min_pulse_width: 最小脉冲宽度 (秒)，默认0.5ms
            max_pulse_width: 最大脉冲宽度 (秒)，默认2.5ms
        """
        # 创建舵机对象（使用默认引脚工厂，无需pigpiod）
        self.servo = Servo(
            pin=pin,
            min_pulse_width=min_pulse_width,
            max_pulse_width=max_pulse_width
        )
        
        # 角度范围设置
        self.min_angle = min_angle
        self.max_angle = max_angle
        self.current_angle = 0  # 初始角度
        
        # 初始化为中间位置
        self.set_angle(0)
    
    def set_angle(self, angle):
        """
        设置舵机角度
        
        参数:
            angle: 目标角度
        """
        # 限制角度在有效范围内
        if angle > self.max_angle:
            angle = self.max_angle
        elif angle < self.min_angle:
            angle = self.min_angle
        
        # 将角度映射到gpiozero的-1到1范围
        mapped_value = self._map_angle_to_value(angle)
        
        # 设置舵机位置
        self.servo.value = mapped_value
        self.current_angle = angle
    
    def get_angle(self):
        """获取当前舵机角度"""
        return self.current_angle
    
    def stop(self):
        """停止舵机控制并释放资源"""
        self.servo.detach()  # 停止PWM信号
    
    def _map_angle_to_value(self, angle):
        """将角度(-90到90)映射到gpiozero的Servo值(-1到1)"""
        return (angle - self.min_angle) / (self.max_angle - self.min_angle) * 2 - 1


if __name__ =='__main__':
    pan = Servo(pin=13, max_angle=90, min_angle=-90)
    tilt = Servo(pin=12, max_angle=30, min_angle=-90)
    panAngle = 0
    tiltAngle = 0
    pan.set_angle(panAngle)
    tilt.set_angle(tiltAngle)
    sleep(1)

    while True:
        for angle in range(0, 90, 1):
            pan.set_angle(angle)
            tilt.set_angle(angle)
            sleep(.01)
        sleep(.5)
        for angle in range(90, -90, -1):
            pan.set_angle(angle)
            tilt.set_angle(angle)
            sleep(.01)
        sleep(.5)
        for angle in range(-90, 0, 1):
            pan.set_angle(angle)
            tilt.set_angle(angle)
            sleep(.01)
        sleep(.5)

